#include "mimic_joint_plugin/MimicJointPlugin.hpp"

using namespace gazebo;

void MimicJointPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  // 初始化 ROS 2 节点
  if (!rclcpp::ok())
  {
    int argc = 0;
    char **argv = nullptr;
    rclcpp::init(argc, argv);
  }
  node_ = std::make_shared<rclcpp::Node>("mimic_joint_plugin");

  RCLCPP_INFO(node_->get_logger(), "[MimicJointPlugin] Loading plugin...");

  // 必须包含 joint_name 和 mimic_joint
  if (!_sdf->HasElement("joint_name") || !_sdf->HasElement("mimic_joint"))
  {
    RCLCPP_ERROR(node_->get_logger(), "[MimicJointPlugin] joint_name and mimic_joint are required.");
    return;
  }

  std::string jointName = _sdf->Get<std::string>("joint_name");
  std::string mimicName = _sdf->Get<std::string>("mimic_joint");
  multiplier_ = _sdf->Get<double>("multiplier", 1.0).first;
  offset_     = _sdf->Get<double>("offset", 0.0).first;

  joint_ = _model->GetJoint(jointName);
  mimic_ = _model->GetJoint(mimicName);

  if (!joint_ || !mimic_)
  {
    RCLCPP_ERROR(node_->get_logger(),
                 "[MimicJointPlugin] Failed to find joints: %s or %s",
                 jointName.c_str(), mimicName.c_str());
    return;
  }

  RCLCPP_INFO(node_->get_logger(),
              "[MimicJointPlugin] Loaded successfully. mimic=%s -> joint=%s, multiplier=%.2f, offset=%.2f",
              mimicName.c_str(), jointName.c_str(), multiplier_, offset_);

  // 绑定更新回调
  updateConn_ = event::Events::ConnectWorldUpdateBegin(
      std::bind(&MimicJointPlugin::OnUpdate, this));
}

void MimicJointPlugin::OnUpdate()
{
  if (!joint_ || !mimic_) return;

  // 获取主关节位置
  double mimic_pos = mimic_->Position(0);

  // 计算目标位置
  double target = multiplier_ * mimic_pos + offset_;

  // 直接位置控制
  joint_->SetPosition(0, target);

  // 可选调试输出，每1000步打印一次
  static int count = 0;
  if (count++ % 1000 == 0)
  {
    RCLCPP_DEBUG(node_->get_logger(),
                 "[MimicJointPlugin] mimic=%.3f target=%.3f",
                 mimic_pos, target);
  }
}

// 注册插件
GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin)
